#include <fhe/prims/SpatialNode.h>
#include <fhe/core/NodeFactory.h>
#include <gtest/gtest.h>

using namespace fhe;
using namespace core;

TEST( MathTest, Rot2d )
{
  ASSERT_EQ( Rot2d(), Rot2d() );
  ASSERT_EQ( Rot2d::fromRadians( Math::PI ), Rot2d::fromDegrees( 180 ) );
}

TEST( MathTest, Vec2d )
{
  ASSERT_EQ( Vec2d(), Vec2d() );
  ASSERT_EQ( Vec2d( 1, 2 ), Vec2d( 1, 2 ) );
  ASSERT_EQ( Vec2d( 1, 2 ), Vec2d( -1, -1 ) + Vec2d( 2, 3 ) );
  ASSERT_EQ( Vec2d( 1, 2 ), Vec2d( 3, 4 ) - Vec2d( 2, 2 ) );
  ASSERT_EQ( Vec2d( 4, 8 ), Vec2d( 2, 4 ) * Vec2d( 2, 2 ) );
  ASSERT_EQ( Vec2d( 1, 2 ), Vec2d( 4, 6 ) / Vec2d( 4, 3 ) );
  ASSERT_TRUE( Math::equal( 11, Vec2d( 1, 2 ).dot( Vec2d( 3, 4 ) ) ) );
  ASSERT_TRUE( Math::equal( 25, Vec2d( 3, 4 ).squaredLength() ) );
  ASSERT_TRUE( Math::equal( 5, Vec2d( 3, 4 ).length() ) );
  ASSERT_EQ( Vec2d( 0, 1 ), Vec2d( 0, 10 ).normalize() );
  ASSERT_TRUE( Math::equal( -1, Vec2d( -1, 1 ).distToLine( Vec2d( 0, -10 ), Vec2d( 0, 10 ) ) ) );
  ASSERT_EQ( Vec2d( -1, 0 ), Vec2d( 0, 1 ) * Rot2d::fromDegrees( 90 ) );
}

template <size_t D, typename T>
void matTest( const Vec<D,T>& v, const Vec<D,T>& t, const Rot<D,T>& r, const Vec<D,T>& s )
{
  typedef Mat<D,T> M;
  M mt = M::translation( t );
  M ms = M::scale( s );
  M mr = M::rotation( r );
  
  ASSERT_EQ( v, M::IDENTITY * v );
  ASSERT_EQ( v + t, mt * v );
  ASSERT_EQ( v * s, ms * v );
  ASSERT_EQ( v * r, mr * v );
  ASSERT_EQ( s * ( ( t + v ) * r ), ms * ( mr * ( mt * v ) ) );
  ASSERT_EQ( s * ( ( t + v ) * r ), ( ms * mr * mt ) * v );
  
  M mti = mt.inverse();
  M msi = ms.inverse();
  M mri = mr.inverse();
  Rot<D,T> ri = r.inverse();
  
  ASSERT_EQ( v, v * r * ri );
  ASSERT_EQ( v - t, mti * v );
  ASSERT_EQ( v / s, msi * v );
  ASSERT_EQ( v * ri, mri * v );
  ASSERT_EQ( ( ( v - t ) * ri ) / s, msi * ( mri * ( mti * v ) ) );
  ASSERT_EQ( ( ( v - t ) * ri ) / s, ( msi * mri * mti ) * v );
}

TEST( MathTest, Mat2d )
{
  matTest<2,double>( Vec2d( 15, 10 ), 
                     Vec2d( -5, 7 ), 
                     Rot2d::fromDegrees( -75 ), 
                     Vec2d( 2, -1 ) );
}

TEST( MathTest, Mat3d )
{
  matTest<3,double>( Vec3d( 15, 10, 5 ), 
                     Vec3d( -5, 7, -9 ), 
                     Rot3d( Vec3d( 1, 2, 3 ), 1.2 ), 
                     Vec3d( 2, -1, 3 ) );
}

TEST( MathTest, SpatialNode2i )
{
  Node::Ptr n1 = NodeFactory::instance().buildNode( "core", "SpatialNode2i" );
  Node::Ptr n2 = NodeFactory::instance().buildNode( "core", "SpatialNode2i" );
  Vec2i p1( 10, 2 );
  Vec2i p2( -15, 4 );
  n1->set( "position", p1 );
  n2->set( "position", p2 );
  n1->addChild( n2 );
  ASSERT_EQ( Mat2i::translation( p1 ), n1->get<Mat2i>( "localTransform" ) );
  ASSERT_EQ( Mat2i::translation( p1 ), n1->get<Mat2i>( "globalTransform" ) );
  ASSERT_EQ( Mat2i::translation( p2 ), n2->get<Mat2i>( "localTransform" ) );
  ASSERT_EQ( Mat2i::translation( p1 + p2 ), n2->get<Mat2i>( "globalTransform" ) );
}

void mathFile( const std::string& filename, const std::string& saveFilename )
{
  Node::Ptr node = Node::load( filename );
  ASSERT_EQ( Vec2d( 1, 2 ), node->get<Vec2d>( "v2d" ) );
  ASSERT_EQ( Vec3d( 3, 4, 5 ), node->get<Vec3d>( "v3d" ) );
  ASSERT_EQ( Rot2d::fromDegrees( 6 ), node->get<Rot2d>( "r2d" ) );
  ASSERT_EQ( Rot3d( Vec3d( 1, 0, 0 ), Math::radians( 90 ) ), node->get<Rot3d>( "r3d" ) );
  ASSERT_EQ( Mat2d::translation( Vec2d( 1, 2 ) ) *
             Mat2d::rotation( Rot2d::fromDegrees( 45 ) ) *
             Mat2d::scale( Vec2d( -1, -2 ) ),
             node->get<Mat2d>( "m2d" ) );
  ASSERT_EQ( Mat3d::translation( Vec3d( 3, 4, 5 ) ) *
             Mat3d::rotation( Rot3d( Vec3d( 0, 1, 0 ), Math::radians( -45 ) ) ) *
             Mat3d::scale( Vec3d( -3, -4, -5 ) ),
             node->get<Mat3d>( "m3d" ) );
  
  if ( !saveFilename.empty() )
  {
    node->save( saveFilename );
  }
}

TEST( MathTest, files )
{
  mathFile( "../tests/MathTest.xml", "MathTest2.xml" );
  mathFile( "MathTest2.xml", "" );
}
